#include <Windows.h>
#include <iostream>
#include "../SDK/include/apx5200sa.h"
#include "SamplingTest.h"
#include <conio.h>
#include <fstream>
#include <iostream>
#include <ctime>
#include <direct.h>
#include "TaskQueue.h"
#include "finally.h"
#include "CscanFileBuilder.h"

void AnalogOutput()
{

    HAD5200BAoParam ao;
    HAD5200BClass daq(0);
    std::ifstream input_file("Data\\sin3000.txt");
    std::string str;
    if (!input_file.fail())
    {
        ao.AoData.reserve(3000);
        for (int i = 0;i < 3000 && !input_file.eof();i++) {
            getline(input_file, str);
            ao.AoData.push_back(atof(str.c_str()));
        }
    }
    try {
        daq.setAoParameter(1, ao);
        daq.startAo(1);
    }
    catch (std::exception exc) {
        std::cout << exc.what() << std::endl;
        return;
    }
    auto final_task = finally([&]() {daq.stopAo(1);});
    double ch1Ao,ch2Ao;
    while (!_kbhit())
    {
        try
        {
            daq.getAoValue(ch1Ao, ch2Ao);
        }
        catch (std::exception ex) {
            std::cout << ex.what() << std::endl;
            break;
        }
        std::cout << "Analog output Ch1 :" << ch1Ao << std::endl;

        Sleep(500);
    }
}

//sc:It is converted to tm
//mill:stores the sc in milliseconds
tm getDateTime(const std::chrono::system_clock::time_point& sc, uint16_t& mill)
{
    auto date_time = std::chrono::system_clock::to_time_t(sc);
    tm tm;
    auto error = localtime_s(&tm, &date_time);
    mill = std::chrono::duration_cast<std::chrono::milliseconds>(
        sc.time_since_epoch()).count() % 1000;
    return tm;
}

void SamplingFromTsvFile() {
	int nBoardIndex = 0;

	std::string filename;
	std::cout << "Input TSV file path." << std::endl;
	std::cin >> filename;

	try {
		HAD5200BClass daq(0);
		SamplingTest test(daq);
		//set parameter from tsv file
		daq.setSamplingParameter(filename.c_str());
		//gets parameter
		auto p = daq.getSamplingParameter();
		SamplingTest::displayParameter(p);
		if (p.Ch1Param.TotalNum == 0) {
			//Infinite sampling
			auto sampling = test.AsyncSampling();
			std::string tmp;
			std::cout << "Input any character and enter key if you stop sampling" << std::endl;
			std::cin >> tmp;
			test.SamplingStop();
			sampling.join();
			std::cout << "Sampling was stopped" << std::endl;
		}
		else {
			//Finite samplig
			auto sampling = test.AsyncSampling();
			sampling.join();
		}
	}
	catch (std::exception ex) {
		std::cout << ex.what() << std::endl;
	}
}

void SamplingFromParameter() {
	int nBoardIndex = 0;
	//#####note:Please set proper parameters,otherwise it will not work normally
	HAD5200BSamplingParam param;
	param.OutputMode = HAD5200BOutputMode::PowerLog;
	param.CaptureSize = 4096;
	param.BankNum = 100;
	param.ResamplingSize = 4096;
	param.ResamplingEnable = HAD5200BEnable::Enable;
	param.FIREnable = HAD5200BEnable::Enable;
	param.CrossThreshold = 2048;
	param.CrossPointSize = 1300;
	param.Delay = 0.0;
	param.InputEdge = HAD5200BInputEdge::EdgeRise;
	param.SyncMode = HAD5200BEnable::Disable;
	param.ClockMask = 0;
	param.Clock = 1000;
	//CH1
	param.Ch1Param.VoltageOffset = 0;
	param.Ch1Param.CaptureNum = 64;
	param.Ch1Param.TotalNum = 0;							//If this value is 0, it paforms infinite sampling
	param.Ch1Param.DiMask = HAD5200BDiMaskFlag::Disable;
	param.Ch1Param.Offset = 0;
	//CH2  Make it the same as Ch1Param
	param.Ch2Param.VoltageOffset = 0;
	param.Ch2Param.CaptureNum = 64;
	param.Ch2Param.TotalNum = 0;  
	param.Ch2Param.DiMask = HAD5200BDiMaskFlag::Disable;
	param.Ch2Param.Offset = 0;

	//get FIR coefficient tap data
	std::string str;
	std::ifstream input_file("Data\\FIR5M-400MBandpath.txt");
	if (!input_file.fail())
	{
		for (int i = 0;i < 51 && !input_file.eof();i++) {
			getline(input_file, str);
			param.FIRWindow[i] = atof(str.c_str());
		}
	}
	input_file.close();
	input_file.clear();
	//get window function data hanning
	//The size of window and Theta is equal to 'ResamplingSize'
	//If you need dispersion compensation, please set 'Theta'
	//Otherwise  set the vector of size of 0 to 'Theta'  
	input_file.open("Data\\han4096.txt");
	if (!input_file.fail())
	{
		param.Window.reserve(4096);
		for (int i = 0;i < 4096 && !input_file.eof();i++) {
			getline(input_file, str);
			param.Window.push_back(atof(str.c_str()));
		}
	}

	try {
		HAD5200BClass daq(0);
		SamplingTest test(daq);
		//set parameter from tsv file
		daq.setSamplingParameter(param);

        //make save folder
		errno = 0;
        if(_mkdir(SamplingTest::SaveFolder))
        {
	        if(errno==ENOENT)
	        {
				std::cout << "No such file or directory " << std::endl;
	        }
        }

		//gets parameter
		auto p = daq.getSamplingParameter();
		SamplingTest::displayParameter(p);
		if (p.Ch1Param.TotalNum == 0) {
			//Infinite sampling
			auto sampling = test.AsyncSampling();
			std::string tmp;
			std::cout << "Input any charactor and enter key if you stop sampling" << std::endl;
			std::cin >> tmp;
			test.SamplingStop();
			sampling.join();
		}
		else {
			//Finite sampling
			auto sampling = test.AsyncSampling();
			sampling.join();
		}

	}
	catch (std::exception ex) {
		std::cout << ex.what() << std::endl;
	}
}

void BScanFromTsvFile()
{
	int nBoardIndex = 0;
	HAD5200PositionSetting position_setting;
	std::string filename;
	std::cout << "Input TSV file path." << std::endl;
	std::cin >> filename;

	try {
		HAD5200BClass daq(0);
		SamplingTest test(daq);
		//set parameter from tsv file
		daq.setSamplingParameter(filename.c_str());
		try
		{
			//position setting has not been configured when occurred exception 
			daq.getPositionSetting(position_setting);
		}
		catch (HAD5200BException exception)
		{
			position_setting.ConvertType = VoltPerPosition;
			position_setting.FocalLength = 60;
			position_setting.ScanRate = 100000;
			position_setting.X.Coefficient = 0.23;
			position_setting.X.Max = 20;
			position_setting.X.Min = -20;
			position_setting.X.MaxSpeed = 10;
			position_setting.X.Reverse = false;
			position_setting.Y.Coefficient = 0.23;
			position_setting.Y.Max = 20;
			position_setting.Y.Min = -20;
			position_setting.Y.MaxSpeed = 10;
			position_setting.Y.Reverse = false;
			daq.setPositionSetting(position_setting);
		}
		
		
		//gets parameter
		auto p = daq.getSamplingParameter();
		HAD5200BscanSetting bscan_setting{
		 HAD5200BScanMode::Normal,
			{-10,0},
			{10,0},
			512,
			0,
			24,
			static_cast<uint32_t>(p.ResamplingSize - 24)
		};
		daq.setBscanSetting(bscan_setting);
		SamplingTest::displayParameter(p);
		if (p.Ch1Param.TotalNum == 0) {
			//Infinite sampling
			auto sampling = test.asyncBScan();
			std::string tmp;
			std::cout << "Input any character and enter key if you stop sampling" << std::endl;
			std::cin >> tmp;
			test.SamplingStop();
			sampling.join();
		}
		else {
			//Finite sampling
			auto sampling = test.asyncBScan();
			sampling.join();
		}
	}
	catch (std::exception ex) {
		std::cout << ex.what() << std::endl;
	}
}

void BScanFromParameter() {
	int nBoardIndex = 0;
	//#####note:Please set proper parameters,otherwise it will not work normally
	HAD5200BSamplingParam param;
	param.CaptureSize = 4096;
	param.BankNum = 32;
	param.ResamplingSize = 4096;
	param.ResamplingEnable = HAD5200BEnable::Enable;
	param.FIREnable = HAD5200BEnable::Enable;
	param.CrossThreshold = 2048;
	param.CrossPointSize = 1300;
	param.Delay = 0.0;
	param.InputEdge = HAD5200BInputEdge::EdgeRise;
	param.SyncMode = HAD5200BEnable::Disable;
	param.ClockMask = 0;
	param.Clock = 1000;
	//CH1
	param.Ch1Param.VoltageOffset = 0;
	param.Ch1Param.CaptureNum = 64;
	param.Ch1Param.TotalNum = 0;							//If this value is 0, it paforms infinite sampling
	param.Ch1Param.DiMask = HAD5200BDiMaskFlag::Disable;
	param.Ch1Param.Offset = 0;
	//CH2  Make it the same as Ch1Param
	param.Ch2Param.VoltageOffset = 0;
	param.Ch2Param.CaptureNum = 64;
	param.Ch2Param.TotalNum = 0;
	param.Ch2Param.DiMask = HAD5200BDiMaskFlag::Disable;
	param.Ch2Param.Offset = 0;
	
	//get FIR coefficient tap data
	std::string str;
	std::ifstream input_file("Data\\FIR5M-400MBandpath.txt");
	if (!input_file.fail())
	{
		for (int i = 0; i < 51 && !input_file.eof(); i++) {
			getline(input_file, str);
			param.FIRWindow[i] = atof(str.c_str());
		}
	}
	input_file.close();
	input_file.clear();
	//get window function data hanning
	//The size of window and Theta is equal to 'ResamplingSize'
	//If you need dispersion compensation, please set 'Theta'
	//Otherwise  set the vector of size of 0 to 'Theta'  
	input_file.open("Data\\han4096.txt");
	if (!input_file.fail())
	{
		param.Window.reserve(4096);
		for (int i = 0; i < 4096 && !input_file.eof(); i++) {
			getline(input_file, str);
			param.Window.push_back(atof(str.c_str()));
		}
	}
	HAD5200PositionSetting position_setting;
	position_setting.ConvertType = VoltPerPosition;
	position_setting.FocalLength = 60;
	position_setting.ScanRate = 100000;
	position_setting.X.Coefficient = 0.23;
	position_setting.X.Max = 20;
	position_setting.X.Min = -20;
	position_setting.X.MaxSpeed = 10;
	position_setting.X.Reverse = false;
	position_setting.Y.Coefficient = 0.23;
	position_setting.Y.Max = 20;
	position_setting.Y.Min = -20;
	position_setting.Y.MaxSpeed = 10;
	position_setting.Y.Reverse = false;

	HAD5200BscanSetting bscan_setting{
		 HAD5200BScanMode::Normal,
			{-10,0},
			{10,0},
			512,
			0,
			24,
		static_cast<uint32_t>(param.ResamplingSize-24)
	};
	try {
		HAD5200BClass daq(0);
		SamplingTest test(daq);
		//set parameter 
		daq.setSamplingParameter(param);
		//set position settings
		daq.setPositionSetting(position_setting);
		//set B scan settings
		daq.setBscanSetting(bscan_setting);

		//make save folder
		_mkdir(SamplingTest::SaveFolder);

		//gets parameter
		auto p = daq.getSamplingParameter();
		SamplingTest::displayParameter(p);
		if (p.Ch1Param.TotalNum == 0) {
			//Infinite sampling
			auto sampling = test.asyncBScan();
			std::string tmp;
			std::cout << "Input any character and enter key if you stop sampling" << std::endl;
			std::cin >> tmp;
			test.SamplingStop();
			sampling.join();
		}
		else {
			//Finite sampling
			auto sampling = test.asyncBScan();
			sampling.join();
		}
	}
	catch (std::exception& ex) {
		std::cout << ex.what() << std::endl;
	}
}

void CreateCscanData()
{
	try {
		std::string filename;
		std::cout << "Input TSV file path." << std::endl;
		std::cin >> filename;
		HAD5200PositionSetting position_setting;
		uint32_t z_length;
		uint32_t z_offset;
		uint32_t y_point_size;
		uint32_t x_point_size;
		float end_y;
		float end_x;
		float start_y;
		float stat_x;
		double resolution;
		HAD5200BClass daq(0);
		//set parameter from tsv file
		daq.setSamplingParameter(filename.c_str());
		try
		{
			//position setting has not been configured when occurred exception 
			daq.getPositionSetting(position_setting);
		}
		catch (HAD5200BException&)
		{
			position_setting.ConvertType = VoltPerPosition;
			position_setting.FocalLength = 60;
			position_setting.ScanRate = 100000;
			position_setting.X.Coefficient = 0.23;
			position_setting.X.Max = 20;
			position_setting.X.Min = -20;
			position_setting.X.MaxSpeed = 10;
			position_setting.X.Reverse = false;
			position_setting.Y.Coefficient = 0.23;
			position_setting.Y.Max = 20;
			position_setting.Y.Min = -20;
			position_setting.Y.MaxSpeed = 10;
			position_setting.Y.Reverse = false;
			daq.setPositionSetting(position_setting);
		}
		auto resampling_size = daq.getSamplingParameter().ResamplingSize;
		//Set measurement setting
		std::cout << "Input start x position [unit:mm]" << std::endl;
		std::cin >> stat_x;
		if(stat_x > position_setting.X.Max || position_setting.X.Min > stat_x)
		{
			std::cout << "Error: invalid value set." << std::endl;
			return;
		}
		std::cout << "Input start y position [unit:mm]" << std::endl;
		std::cin >> start_y;
		if (start_y > position_setting.Y.Max || position_setting.Y.Min > start_y)
		{
			std::cout << "Error: invalid value set." << std::endl;
			return;
		}
		std::cout << "Input end x position [unit:mm]" << std::endl;
		std::cin >> end_x;
		if (end_x > position_setting.X.Max || position_setting.X.Min > end_x)
		{
			std::cout << "Error: invalid value set." << std::endl;
			return;
		}
		std::cout << "Input end y position [unit:mm]" << std::endl;
		std::cin >> end_y;
		if (end_y > position_setting.Y.Max || position_setting.Y.Min > end_y)
		{
			std::cout << "Error: invalid value set." << std::endl;
			return;
		}
		std::cout << "Input number of x pixel" << std::endl;
		std::cin >> x_point_size;
		if (x_point_size == 0)
		{
			std::cout << "Error: invalid value set." << std::endl;
			return;
		}
		std::cout << "Input number of y pixel" << std::endl;
		std::cin >> y_point_size;
		if (y_point_size == 0)
		{
			std::cout << "Error: invalid value set." << std::endl;
			return;
		}
		std::cout << "Input depth offset" << std::endl;
		std::cin >> z_offset;
		std::cout << "Input depth length" << std::endl;
		std::cin >> z_length;
		if (z_length == 0)
		{
			std::cout << "Error: invalid value set." << std::endl;
			return;
		}
		if( z_length == 0 || z_length + z_offset > static_cast<uint32_t>(resampling_size) / 2)
		{
			std::cout << "Error: [depth offset] + [depth length] must be less than equal [resamplingsize]/2" << std::endl;
			return;
		}
		std::cout << "Input depth pixel resolution[unit: mm /pixel]" << std::endl;
		std::cin >> resolution;
		if (resolution <= 0)
		{
			std::cout << "Error: invalid value set." << std::endl;
			return;
		}
		std::string save_folder = "3DData"; //Specify 3D data saving folder
		HAD5200CscanSetting cscan_setting{
		 HAD5200CScanMode::RasterXLine,
			{stat_x,start_y},
			{end_x,end_y},
			x_point_size,
			y_point_size,
			z_offset,
			z_length,
			0,
			0
		};
		daq.setCscanSetting(cscan_setting);

		HAD5200CscanSetting configur;
		daq.getCscanSetting(configur);
		cscan_points<double> scale{
			abs(configur.Start.X - configur.End.X),
			abs(configur.Start.Y - configur.End.Y),
			resolution * configur.ZLength
		};
		cscan_info info{
			{configur.XPointNum,configur.YPointNum,configur.ZLength},
			scale,
			0
		};
		CscanFileBuilder builder{ save_folder,info };
		std::vector<std::vector<float>> buffer(CscanFileBuilder::max_buffer_size);
		uint32_t buffer_index = 0;
		for (auto& line_buffer : buffer)
		{
			line_buffer = std::vector<float>(info.size.x * info.size.z);
		}
		std::cout << "Start Cscan" << std::endl;
		daq.startCscan();
		auto final = finally([&]()
			{
				builder.cancel();
				try
				{
					daq.stopCscan();
				}
				catch (HAD5200BException&)
				{
					//ignore
				}
			});
		auto number_of_line = info.size.y;
		auto segment_index = 0U;
		APX5200SA_DATA sampling_status;
		while (1)
		{
			auto c_scan_size = static_cast<uint32_t>(buffer[buffer_index].size());
			auto depth_size = static_cast<uint32_t>(info.size.z);
			daq.waitCscanData(1000, sampling_status, &buffer[buffer_index][0], c_scan_size, depth_size, segment_index);
			if (sampling_status.status != 0 && sampling_status.status !=1)
			{
				std::cout << "Occur abnormal status. Status value =  " << sampling_status.status << std::endl;
			}
			if (!builder.input_new_line(buffer[buffer_index]))
			{
				std::cout << "Buffer overflow" << std::endl;
				return;
			}
			if (segment_index == number_of_line - 1)
			{
				break;
			}
			buffer_index++;
			if (buffer_index >= buffer.size())
				buffer_index = 0U;
		}
		//wait completion of saving
		while (builder.get_status() == 1)
		{
			std::cout << "Wait saving data process. "  << std::endl;
			std::this_thread::sleep_for(std::chrono::milliseconds(100));
		}
		if (builder.get_status() == 2)
		{
			std::cout << "Data saving is completed." << std::endl;
		}
		else if (builder.get_status() == 3)
		{
			std::cout << "Data saving process is canceled. " << builder.get_buffer_size() << std::endl;
		}
		else
		{
			std::cout << "Error occurred in data saving process. " << builder.get_buffer_size() << std::endl;
		}
	}
	catch (HAD5200BException& ex)
	{
		std::cout << ex.what() << std::endl;
	}
	catch (std::exception& ex) {
		std::cout << ex.what() << std::endl;
	}
	
}




SamplingTest::SamplingTest(HAD5200BClass & d) :daq_(d)
{};

void SamplingTest::rawSamplingLoop()
{

    long times = -1;
    uint32_t count = 0;
    APX5200SA_DATA ch1, ch2;
    const auto param = daq_.getSamplingParameter();

	BufferedQueue<Rawdata> file(DataNumber, SpoolerBuffer, saveRawData);
    if (param.Ch1Param.TotalNum != 0)
    {
        times = param.Ch1Param.TotalNum / param.Ch1Param.CaptureNum;
    }

    file.startConsumer();
    auto stop = finally([&] {
        daq_.samplingStop();
        file.stopConsumer();
        std::cout << "sampling stoped" << std::endl;
    }); //finally
    int32_t loop_cnt = 0;
    std::cout << "sampling start" << std::endl;
    daq_.samplingStart();
    while (true)
    {
        try
        {
            daq_.waitSampling(1000, ch1, ch2);
        }
        catch (std::exception ex) {
            std::cout << ex.what() << std::endl;
            break;
        }
        if (ch1.status != 0) {
            std::cout << " CH1 is abnormal status value=" << ch1.status << std::endl;
        }
        if (ch2.status != 0) {
            std::cout << " CH2 is abnormal status value=" << ch2.status << std::endl;
        }
        if (!file.post(Rawdata(ch1.pBuffer,ch2.pBuffer, ch1.SegmentSize))) {
            std::cout << " Save buffer is full"  << std::endl;
        }
        loop_cnt++;
        if (sampling_stop_ || (times != -1 && loop_cnt >= times))
            break;
    }
}

void SamplingTest::resamplingLoop()
{
    long times = -1;
    uint32_t count = 0;
    APX5200SA_DATA ch1, ch2;
    const auto param = daq_.getSamplingParameter();

	BufferedQueue<ResamplingData> file(DataNumber, SpoolerBuffer, saveResamplingData);
    if (param.Ch1Param.TotalNum != 0)
    {
        times = param.Ch1Param.TotalNum / param.Ch1Param.CaptureNum;
    }

    file.startConsumer();
    auto stop = finally([&] {
        daq_.samplingStop();
        file.stopConsumer();
        std::cout << "sampling stoped" << std::endl;
    }); //finally
    int32_t loop_cnt = 0;
    std::cout << "sampling start" << std::endl;
    daq_.samplingStart();
    while (true)
    {
        try
        {
            daq_.waitSampling(1000, ch1, ch2);
        }
        catch (std::exception ex) {
            std::cout << ex.what() << std::endl;
            break;
        }
        if (ch1.status != 0) {
            std::cout << " CH1 is abnormal status value=" << ch1.status << std::endl;
        }
        if (!file.post(ResamplingData(ch1.pBuffer, ch1.SegmentSize))) {
            std::cout << " Save buffer is full" << std::endl;
        }
        loop_cnt++;
        if (sampling_stop_ || (times != -1 && loop_cnt >= times))
            break;
    }
}

void SamplingTest::fftLoop()
{
    long times = -1;
    uint32_t count = 0;
    APX5200SA_DATA ch1, ch2;
    auto param = daq_.getSamplingParameter();

    BufferedQueue<FFTdata> file(DataNumber, SpoolerBuffer, saveFFTdata);
    if (param.Ch1Param.TotalNum != 0)
    {
        times = param.Ch1Param.TotalNum / param.Ch1Param.CaptureNum;
    }

    file.startConsumer();
    auto stop = finally([&] {
        daq_.samplingStop();
        file.stopConsumer();
        std::cout << "sampling stopped" << std::endl;
    }); //finally
    int32_t loop_cnt = 0;
    std::cout << "sampling start" << std::endl;
    daq_.samplingStart();
    while (true)
    {
        try
        {
            daq_.waitSampling(1000, ch1, ch2);
        }
        catch (std::exception ex) {
            std::cout << ex.what() << std::endl;
            break;
        }
        if (ch1.status != 0) {
            std::cout << " CH1 is abnormal status value=" << ch1.status << std::endl;
        }
        if (ch2.status != 0) {
            std::cout << " CH2 is abnormal status value=" << ch2.status << std::endl;
        }
        if (!file.post(FFTdata(reinterpret_cast<float *>(ch1.pBuffer),
                reinterpret_cast<float *>(ch2.pBuffer), ch1.SegmentSize/2))) {
            std::cout << " Save buffer is full" << std::endl;
        }
        loop_cnt++;
        if (sampling_stop_ || (times != -1 && loop_cnt >= times))
            break;
    }
}

void SamplingTest::aScanLoop()
{

    long times = -1;
    uint32_t count = 0;
    APX5200SA_DATA ch1, ch2;
    auto param = daq_.getSamplingParameter();

    BufferedQueue<Ascan> file(DataNumber, SpoolerBuffer, saveAScan);
    if (param.Ch1Param.TotalNum != 0)
    {
        times = param.Ch1Param.TotalNum / param.Ch1Param.CaptureNum;
    }

    file.startConsumer();
    auto stop = finally([&] {
        daq_.samplingStop();
        file.stopConsumer();
        std::cout << "sampling stopped" << std::endl;
    }); //finally
    int32_t loop_cnt = 0;
    std::cout << "sampling start" << std::endl;
    daq_.samplingStart();
    while (true)
    {
        try
        {
            daq_.waitSampling(1000, ch1, ch2);
        }
        catch (std::exception& ex) {
            std::cout << ex.what() << std::endl;
            break;
        }
        if (ch1.status != 0) {
            std::cout << " CH1 is abnormal status value=" << ch1.status << std::endl;
        }
        if (!file.post(Ascan(reinterpret_cast<float *>(ch1.pBuffer), ch1.SegmentSize / 2))) {
            std::cout << "Save buffer is full " << std::endl;
        }
        loop_cnt++;
        if (sampling_stop_ || (times != -1 && loop_cnt >= times))
            break;
    }
}
void SamplingTest::bScanLoop()
{
	const auto image_save_interval = 1000.0; //[ms]
	long times = -1;

	float* bscan_data=nullptr;
	auto param = daq_.getSamplingParameter();
	HAD5200BscanSetting bscan_setting;
	daq_.getBscanSetting(bscan_setting);
	uint32_t depth_size = param.ResamplingSize / 2 - bscan_setting.DepthOffset;
	uint32_t size = depth_size * bscan_setting.PointNum;
	BScanImage image(512, 512);
	image.setMaxIntensity(30);
	image.setMinIntensity(-20);
	TaskQueue<BScan> file(10, saveImage);
	uint16_t mil;
	uint32_t count = 0;
	WCHAR save_folder[128];
	auto tm = getDateTime(std::chrono::system_clock::now(), mil);
	
	if (param.Ch1Param.TotalNum != 0)
	{
		times = param.Ch1Param.TotalNum / param.Ch1Param.CaptureNum;
	}
	//make folder
	swprintf_s(save_folder, 128,
		L"%s/%02d%02d%02d_%02d%02d%02d", SaveFolder_w,
		tm.tm_year - 100, tm.tm_mon + 1, tm.tm_mday, tm.tm_hour, tm.tm_min, tm.tm_sec);
	if(_wmkdir(save_folder ) != 0)
	{
		std::cout << "failed to make directory" << std::endl;
		return;
	}

	file.startConsumer();
	auto stop = finally([&] {
		daq_.stopBScan();
		file.stopConsumer();
		std::cout << "sampling stopped" << std::endl;
	}); //finally
	long loop_cnt = 0;
	std::cout << "sampling start" << std::endl;
	daq_.startBScan();
	auto fast = true;
	auto start = std::chrono::system_clock::now();
	APX5200SA_DATA status;
	while (true)
	{
		try
		{
			daq_.waitBscanData(1000, status, bscan_data, size, depth_size);
		}
		catch (HAD5200BException ex) {
			std::cout << ex.what() << std::endl;
			break;
		}
		if (status.status != 0) {
			std::cout << " CH1 is abnormal status value=" << status.status << std::endl;
		}
		loop_cnt++;
		if (sampling_stop_ || (times != -1 && loop_cnt >= times))
			break;
		//file save
		auto now = std::chrono::system_clock::now();
		auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(now - start).count();
		if(fast || elapsed > image_save_interval)
		{
			start = now;
			fast = false;
			swprintf_s(save_folder, 128, L"%s/%02d%02d%02d_%02d%02d%02d/%04d.png", SaveFolder_w,
				tm.tm_year - 100, tm.tm_mon + 1, tm.tm_mday, tm.tm_hour, tm.tm_min, tm.tm_sec, count++);
			file.post(BScan{bscan_data,size,depth_size,image, std::wstring(save_folder) });
		}
	}
}

std::thread  SamplingTest::AsyncSampling() {
	//sampling start
	sampling_stop_ = false;

    auto param = daq_.getSamplingParameter();
    auto outputmode = param.OutputMode;

    std::thread loop;
    switch (outputmode)
    {
    case HAD5200BOutputMode::Row:
        loop = std::thread{ [this] {rawSamplingLoop(); } };
        break;
    case HAD5200BOutputMode::Resample:
        loop = std::thread{ [this] { resamplingLoop(); } };
        break;
    case HAD5200BOutputMode::Fft:
        loop = std::thread{ [this] { fftLoop(); } };
        break;
    case HAD5200BOutputMode::Power:
        loop = std::thread{ [this] { aScanLoop(); } };
        break;
    case HAD5200BOutputMode::PowerLog:
        loop = std::thread{ [this] { aScanLoop(); } };
        break;
    default:
        break;
    }
	return loop;
}


void SamplingTest::SamplingStop()
{
	sampling_stop_ = true;
}

std::thread SamplingTest::asyncBScan()
{
	return std::thread(&SamplingTest::bScanLoop,this);
}

const void SamplingTest::displayParameter(const HAD5200BSamplingParam & p)
{
	std::cout << "##########Sampling Parameter##########" <<  std::endl;
	std::cout << "OutputMode        :" << getHAD5200BOutputModeString(p.OutputMode) << std::endl;
	std::cout << "CaptureSize       :" << p.CaptureSize << std::endl;
	std::cout << "BankNum           :" << p.BankNum << std::endl;
	std::cout << "ResamplingSize    :" << p.ResamplingSize << std::endl;
	std::cout << "CrossPointSize    :" << p.CrossPointSize << std::endl;
	std::cout << "CaptureNum        :" << p.Ch1Param.CaptureNum << std::endl;
	std::cout << "TotalNum          :" << p.Ch1Param.TotalNum << std::endl;
	std::cout << "Offset            :" << p.Ch1Param.Offset << std::endl;
	std::cout << "######################################" << std::endl;
}

const std::string SamplingTest::getHAD5200BOutputModeString(const HAD5200BOutputMode& mode) noexcept
{
	switch (mode)
	{
	case HAD5200BOutputMode::Row:
		return "Row";
	case HAD5200BOutputMode::Resample:
		return "Resample";
	case HAD5200BOutputMode::Fft:
		return "FFT";
	case HAD5200BOutputMode::Power:
		return "Power";
	case HAD5200BOutputMode::PowerLog:
		return "PowerLog";
	case HAD5200BOutputMode::FftHalf:
		return "PowerLog Half";
	case HAD5200BOutputMode::PowerHalf:
		return "PowerLog Half";
	case HAD5200BOutputMode::LogHalf:
		return "PowerLog Half";
	case HAD5200BOutputMode::FftQuarter:
		return "PowerLog Quarter";
	case HAD5200BOutputMode::PowerQuarter:
		return "PowerLog Quarter";
	case HAD5200BOutputMode::LogQuarter:
		return "PowerLog Quarter";
		
	default:
		return "Unkown";
	}
}

const std::string SamplingTest::getHAD5200BEnableString(const HAD5200BEnable& mode) noexcept
{
	switch (mode)
	{
	case HAD5200BEnable::Disable:
		return "Disable";
	case HAD5200BEnable::Enable:
		return "Enable";
	default:
		return "Unkown";
	}
}

void SamplingTest::saveRawData(std::vector<Rawdata>& buff)
{
    using namespace std::chrono;
    if (buff.empty())
        return;

    uint16_t mil;
    char path_buff[128];
    auto tm = getDateTime((--buff.end())->timeStamp, mil);
    sprintf_s(&path_buff[0], 128, "%s/Raw%02d%02d%02d_%02d%02d%02d_%03d.csv", SaveFolder,
        tm.tm_year - 100, tm.tm_mon + 1, tm.tm_mday, tm.tm_hour, tm.tm_min, tm.tm_sec, mil);

    auto output = std::ofstream(path_buff);
    if (!output)return;

    auto maxsize = (6 * buff.begin()->ch1.size() + 22) * 2; //One line of data must be within this size
    auto buf = std::unique_ptr<char[]>(new char[maxsize]);
    for (const auto& ascan : buff)
    {
        tm = getDateTime(ascan.timeStamp, mil);
        int32_t size = sprintf_s(buf.get(), maxsize, "%02d/%02d/%02d %02d:%02d:%02d.%03d,CH1",
            tm.tm_year - 100, tm.tm_mon + 1, tm.tm_mday, tm.tm_hour, tm.tm_min, tm.tm_sec, mil);
        for (const auto& d : ascan.ch1)
        {
            size += sprintf_s(buf.get() + size, maxsize - size, ",%d", d);
        }
        size += sprintf_s(buf.get() + size, maxsize - size, "\n");
        size += sprintf_s(buf.get() + size, maxsize - size, "%02d/%02d/%02d %02d:%02d:%02d.%03d,CH2",
            tm.tm_year - 100, tm.tm_mon + 1, tm.tm_mday, tm.tm_hour, tm.tm_min, tm.tm_sec, mil);
        for (const auto& d : ascan.ch2)
        {
            size += sprintf_s(buf.get() + size, maxsize - size, ",%d", d);
        }
        size += sprintf_s(buf.get() + size, maxsize - size, "\n\0");
        output << buf.get();
    }
    output.close();
}

void SamplingTest::saveResamplingData(std::vector<ResamplingData>& buff)
{
    using namespace std::chrono;
    if (buff.empty())
        return;

    uint16_t mil;
    char path_buff[128];
    auto tm = getDateTime((--buff.end())->timeStamp, mil);
    sprintf_s(&path_buff[0], 128, "%s/Resampling%02d%02d%02d_%02d%02d%02d_%03d.csv", SaveFolder,
        tm.tm_year - 100, tm.tm_mon + 1, tm.tm_mday, tm.tm_hour, tm.tm_min, tm.tm_sec, mil);

    auto output = std::ofstream(path_buff);
    if (!output)return;

    auto maxsize = 6 * buff.begin()->data.size() + 22; //One line of data must be within this size
    auto buf = std::unique_ptr<char[]>(new char[maxsize]);
    for (const auto& ascan : buff)
    {
        tm = getDateTime(ascan.timeStamp, mil);
        int32_t size = sprintf_s(buf.get(), maxsize, "%02d/%02d/%02d %02d:%02d:%02d.%03d",
            tm.tm_year - 100, tm.tm_mon + 1, tm.tm_mday, tm.tm_hour, tm.tm_min, tm.tm_sec, mil);
        for (const auto& d : ascan.data)
        {
            size += sprintf_s(buf.get() + size, maxsize - size, ",%d", d);
        }
        size += sprintf_s(buf.get() + size, maxsize - size, "\n\0");
        output << buf.get();
    }
    output.close();
}

void SamplingTest::saveFFTdata(std::vector<FFTdata>& buff)
{
    using namespace std::chrono;
    if (buff.empty())
        return;

    uint16_t mil;
    char path_buff[128];
    auto tm = getDateTime((--buff.end())->timeStamp, mil);
    sprintf_s(&path_buff[0], 128, "%s/FFT%02d%02d%02d_%02d%02d%02d_%03d.csv", SaveFolder,
        tm.tm_year - 100, tm.tm_mon + 1, tm.tm_mday, tm.tm_hour, tm.tm_min, tm.tm_sec, mil);

    auto output = std::ofstream(path_buff);
    if (!output)return;

    const auto max_size = (20 * buff.begin()->real.size() + 22) * 2; //One line of data must be within this size
    auto buf = std::unique_ptr<char[]>(new char[max_size]);
    for (const auto& ascan : buff)
    {
        tm = getDateTime(ascan.timeStamp, mil);
        int32_t size = sprintf_s(buf.get(), max_size, "%02d/%02d/%02d %02d:%02d:%02d.%03d,REAL",
            tm.tm_year - 100, tm.tm_mon + 1, tm.tm_mday, tm.tm_hour, tm.tm_min, tm.tm_sec, mil);
        for (const auto& d : ascan.real)
        {
            size += sprintf_s(buf.get() + size, max_size - size, ",%f", d);
        }
        size += sprintf_s(buf.get() + size, max_size - size, "\n");
        size += sprintf_s(buf.get() + size, max_size - size, "%02d/%02d/%02d %02d:%02d:%02d.%03d,IMAGINARY",
            tm.tm_year - 100, tm.tm_mon + 1, tm.tm_mday, tm.tm_hour, tm.tm_min, tm.tm_sec, mil);
        for (const auto& d : ascan.imaginary)
        {
            size += sprintf_s(buf.get() + size, max_size - size, ",%f", d);
        }
        size += sprintf_s(buf.get() + size, max_size - size, "\n\0");
        output << buf.get();
    }
    output.close();
}

void SamplingTest::saveAScan(std::vector<Ascan>& buff)
{
    using namespace std::chrono;
    if (buff.empty())
        return;

    uint16_t mil;
    char path_buff[128];
    auto tm = getDateTime((--buff.end())->timeStamp, mil);
    sprintf_s(&path_buff[0], 128, "%s/AScan%02d%02d%02d_%02d%02d%02d_%03d.csv", SaveFolder,
        tm.tm_year - 100, tm.tm_mon + 1, tm.tm_mday, tm.tm_hour, tm.tm_min, tm.tm_sec, mil);

    auto output = std::ofstream(path_buff);
    if (!output)return;

    auto maxsize = 20 * buff.begin()->data.size() + 22; //One line of data must be within this size
    auto buf = std::unique_ptr<char[]>(new char[maxsize]);
    for (const auto& ascan : buff)
    {
        tm = getDateTime(ascan.timeStamp, mil);
        int32_t size = sprintf_s(buf.get(), maxsize, "%02d/%02d/%02d %02d:%02d:%02d.%03d",
            tm.tm_year - 100, tm.tm_mon + 1, tm.tm_mday, tm.tm_hour, tm.tm_min, tm.tm_sec, mil);
        for (const auto& d : ascan.data)
        {
            size += sprintf_s(buf.get() + size, maxsize - size, ",%f", d);
        }
		size += sprintf_s(buf.get() + size, maxsize - size, "\n\0");
        output << buf.get();
    }
    output.close();

}

void SamplingTest::saveImage(BScan&& bscan)
{
	using namespace std::chrono;
	if (bscan.data.empty())
		return;
	auto ptr = bscan.data.data();
	const auto size = bscan.data.size();
	const auto depth = bscan.depth_size;
	bscan.image->setBscan(ptr, size, depth);
	bscan.image->outputImage(bscan.file_name.c_str());
}




